/*
 ------------------------------------------------------------------------------
 Copyright (C) 1996-2001 Id Software, Inc.

 This file is part of the Quake source code.

 The Quake source code is free software; you can redistribute it and/or
 modify it under the terms of the GNU General Public License as published by
 the Free Software Foundation; either version 2 of the License, or (at your
 option) any later version.

 The Quake source code is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
 more details.

 You should have received a copy of the GNU General Public License along with
 the Quake source code; if not, write to the Free Software Foundation, Inc.,
 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 ------------------------------------------------------------------------------
*/


//
// matrix.cpp - matrix math library
//


#include "../shared.h"


glqMat3		mat3_identity(1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f);
glqMat4		mat4_identity(1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f);


/*
 ==============================================================================

 3x3 MATRIX

 ==============================================================================
*/


/*
 ==================
 glqMat3::Inverse
 ==================
*/
glqMat3 glqMat3::Inverse (void) const {

	glqMat3	inverse;
	float	det, invDet;
	float	d00, d10, d20;

	d00 = m[1][1] * m[2][2] - m[1][2] * m[2][1];
	d10 = m[1][2] * m[2][0] - m[1][0] * m[2][2];
	d20 = m[1][0] * m[2][1] - m[1][1] * m[2][0];

	det = m[0][0] * d00 + m[0][1] * d10 + m[0][2] * d20;
	if (det == 0.0f){
		inverse = *this;
		return inverse;
	}

	invDet = 1.0f / det;

	inverse[0][0] = d00 * invDet;
	inverse[0][1] = (m[0][2] * m[2][1] - m[0][1] * m[2][2]) * invDet;
	inverse[0][2] = (m[0][1] * m[1][2] - m[0][2] * m[1][1]) * invDet;
	inverse[1][0] = d10 * invDet;
	inverse[1][1] = (m[0][0] * m[2][2] - m[0][2] * m[2][0]) * invDet;
	inverse[1][2] = (m[0][2] * m[1][0] - m[0][0] * m[1][2]) * invDet;
	inverse[2][0] = d20 * invDet;
	inverse[2][1] = (m[0][1] * m[2][0] - m[0][0] * m[2][1]) * invDet;
	inverse[2][2] = (m[0][0] * m[1][1] - m[0][1] * m[1][0]) * invDet;

	return inverse;
}

/*
 ==================
 glqMat3::InverseSelf
 ==================
*/
void glqMat3::InverseSelf (void){

	float	det, invDet;
	float	d00, d10, d20;
	float	d01, d11, d21;
	float	d02, d12, d22;

	d00 = m[1][1] * m[2][2] - m[1][2] * m[2][1];
	d10 = m[1][2] * m[2][0] - m[1][0] * m[2][2];
	d20 = m[1][0] * m[2][1] - m[1][1] * m[2][0];

	det = m[0][0] * d00 + m[0][1] * d10 + m[0][2] * d20;
	if (det == 0.0f)
		return;

	invDet = 1.0f / det;

	d01 = m[0][2] * m[2][1] - m[0][1] * m[2][2];
	d02 = m[0][1] * m[1][2] - m[0][2] * m[1][1];
	d11 = m[0][0] * m[2][2] - m[0][2] * m[2][0];
	d12 = m[0][2] * m[1][0] - m[0][0] * m[1][2];
	d21 = m[0][1] * m[2][0] - m[0][0] * m[2][1];
	d22 = m[0][0] * m[1][1] - m[0][1] * m[1][0];

	m[0][0] = d00 * invDet;
	m[0][1] = d01 * invDet;
	m[0][2] = d02 * invDet;
	m[1][0] = d10 * invDet;
	m[1][1] = d11 * invDet;
	m[1][2] = d12 * invDet;
	m[2][0] = d20 * invDet;
	m[2][1] = d21 * invDet;
	m[2][2] = d22 * invDet;
}

/*
 ==================
 glqMat3::ToAngles
 ==================
*/
glqAngles glqMat3::ToAngles (void) const {

	glqAngles	angles;
	float		s, c, theta;

	if (m[0][2] > 1.0f)
		s = 1.0f;
	else if (m[0][2] < -1.0f)
		s = -1.0f;
	else
		s = m[0][2];

	theta = -glqMath::ASin(s);
	c = glqMath::Cos(theta);

	if (c > 8192.0f * M_EPSILON){
		angles.pitch = RAD2DEG(theta);
		angles.yaw = RAD2DEG(glqMath::ATan(m[0][1], m[0][0]));
		angles.roll = RAD2DEG(glqMath::ATan(m[1][2], m[2][2]));
	}
	else {
		angles.pitch = RAD2DEG(theta);
		angles.yaw = RAD2DEG(-glqMath::ATan(m[1][0], m[1][1]));
		angles.roll = 0.0f;
	}

	return angles;
}

/*
 ==================
 glqMat3::ToQuat
 ==================
*/
glqQuat glqMat3::ToQuat (void) const {

	glqQuat	quat;
	float	trace, scale, s;
	int		next[3] = {1, 2, 0};
	int		i, j, k;

	trace = m[0][0] + m[1][1] + m[2][2];

	if (trace > 0.0f){
		s = glqMath::Sqrt(trace + 1.0f);
		scale = 0.5f / s;

		quat.x = (m[2][1] - m[1][2]) * scale;
		quat.y = (m[0][2] - m[2][0]) * scale;
		quat.z = (m[1][0] - m[0][1]) * scale;
		quat.w = s * 0.5f;
	}
	else {
		i = 0;
		if (m[1][1] > m[0][0])
			i = 1;
		if (m[2][2] > m[i][i])
			i = 2;

		j = next[i];
		k = next[j];

		s = glqMath::Sqrt((m[i][i] - (m[j][j] + m[k][k])) + 1.0f);
		scale = 0.5f / s;

		quat[i] = s * 0.5f;
		quat[j] = (m[j][i] + m[i][j]) * scale;
		quat[k] = (m[k][i] + m[i][k]) * scale;
		quat[3] = (m[k][j] - m[j][k]) * scale;
	}

	return quat;
}

/*
 ==================
 glqMat3::ToMat4
 ==================
*/
glqMat4 glqMat3::ToMat4 (void) const {

	glqMat4	mat;

	mat[0][0] = m[0][0];
	mat[0][1] = m[1][0];
	mat[0][2] = m[2][0];
	mat[0][3] = 0.0f;
	mat[1][0] = m[0][1];
	mat[1][1] = m[1][1];
	mat[1][2] = m[2][1];
	mat[1][3] = 0.0f;
	mat[2][0] = m[0][2];
	mat[2][1] = m[1][2];
	mat[2][2] = m[2][2];
	mat[2][3] = 0.0f;
	mat[3][0] = 0.0f;
	mat[3][1] = 0.0f;
	mat[3][2] = 0.0f;
	mat[3][3] = 1.0f;

	return mat;
}

/*
 ==================
 glqMat3::ToString

 This is just a convenience function for printing matrices
 ==================
*/
const char *glqMat3::ToString (int precision) const {

	return Str_FromFloatArray(ToFloatPtr(), 9, precision);
}


/*
 ==============================================================================

 4x4 MATRIX

 ==============================================================================
*/


/*
 ==================
 glqMat4::Inverse
 ==================
*/
glqMat4 glqMat4::Inverse (void) const {

	glqMat4	inverse;
	float	det, invDet;
	float	d00, d10, d20, d30;
	float	d01, d11, d21, d31;
	float	d02, d12, d22, d32;
	float	d03, d13, d23, d33;
	float	t0, t1, t2, t3, t4, t5;

	t0 = m[2][0] * m[3][1] - m[2][1] * m[3][0];
    t1 = m[2][0] * m[3][2] - m[2][2] * m[3][0];
    t2 = m[2][0] * m[3][3] - m[2][3] * m[3][0];
    t3 = m[2][1] * m[3][2] - m[2][2] * m[3][1];
    t4 = m[2][1] * m[3][3] - m[2][3] * m[3][1];
    t5 = m[2][2] * m[3][3] - m[2][3] * m[3][2];

    d00 =  (t5 * m[1][1] - t4 * m[1][2] + t3 * m[1][3]);
    d10 = -(t5 * m[1][0] - t2 * m[1][2] + t1 * m[1][3]);
    d20 =  (t4 * m[1][0] - t2 * m[1][1] + t0 * m[1][3]);
    d30 = -(t3 * m[1][0] - t1 * m[1][1] + t0 * m[1][2]);

    det = m[0][0] * d00 + m[0][1] * d10 + m[0][2] * d20 + m[0][3] * d30;
	if (det == 0.0f){
		inverse = *this;
		return inverse;
	}

	invDet = 1.0f / det;

    d01 = -(t5 * m[0][1] - t4 * m[0][2] + t3 * m[0][3]);
    d11 =  (t5 * m[0][0] - t2 * m[0][2] + t1 * m[0][3]);
    d21 = -(t4 * m[0][0] - t2 * m[0][1] + t0 * m[0][3]);
    d31 =  (t3 * m[0][0] - t1 * m[0][1] + t0 * m[0][2]);

    t0 = m[1][0] * m[3][1] - m[1][1] * m[3][0];
    t1 = m[1][0] * m[3][2] - m[1][2] * m[3][0];
    t2 = m[1][0] * m[3][3] - m[1][3] * m[3][0];
    t3 = m[1][1] * m[3][2] - m[1][2] * m[3][1];
    t4 = m[1][1] * m[3][3] - m[1][3] * m[3][1];
    t5 = m[1][2] * m[3][3] - m[1][3] * m[3][2];

	d02 =  (t5 * m[0][1] - t4 * m[0][2] + t3 * m[0][3]);
    d12 = -(t5 * m[0][0] - t2 * m[0][2] + t1 * m[0][3]);
    d22 =  (t4 * m[0][0] - t2 * m[0][1] + t0 * m[0][3]);
    d32 = -(t3 * m[0][0] - t1 * m[0][1] + t0 * m[0][2]);

    t0 = m[2][1] * m[1][0] - m[2][0] * m[1][1];
    t1 = m[2][2] * m[1][0] - m[2][0] * m[1][2];
    t2 = m[2][3] * m[1][0] - m[2][0] * m[1][3];
    t3 = m[2][2] * m[1][1] - m[2][1] * m[1][2];
    t4 = m[2][3] * m[1][1] - m[2][1] * m[1][3];
    t5 = m[2][3] * m[1][2] - m[2][2] * m[1][3];

    d03 = -(t5 * m[0][1] - t4 * m[0][2] + t3 * m[0][3]);
    d13 =  (t5 * m[0][0] - t2 * m[0][2] + t1 * m[0][3]);
    d23 = -(t4 * m[0][0] - t2 * m[0][1] + t0 * m[0][3]);
    d33 =  (t3 * m[0][0] - t1 * m[0][1] + t0 * m[0][2]);

	inverse[0][0] = d00 * invDet;
	inverse[0][1] = d01 * invDet;
	inverse[0][2] = d02 * invDet;
	inverse[0][3] = d03 * invDet;
	inverse[1][0] = d10 * invDet;
	inverse[1][1] = d11 * invDet;
	inverse[1][2] = d12 * invDet;
	inverse[1][3] = d13 * invDet;
	inverse[2][0] = d20 * invDet;
	inverse[2][1] = d21 * invDet;
	inverse[2][2] = d22 * invDet;
	inverse[2][3] = d23 * invDet;
	inverse[3][0] = d30 * invDet;
	inverse[3][1] = d31 * invDet;
	inverse[3][2] = d32 * invDet;
	inverse[3][3] = d33 * invDet;

	return inverse;
}

/*
 ==================
 glqMat4::InverseSelf
 ==================
*/
void glqMat4::InverseSelf (void){

	float	det, invDet;
	float	d00, d10, d20, d30;
	float	d01, d11, d21, d31;
	float	d02, d12, d22, d32;
	float	d03, d13, d23, d33;
	float	t0, t1, t2, t3, t4, t5;

	t0 = m[2][0] * m[3][1] - m[2][1] * m[3][0];
    t1 = m[2][0] * m[3][2] - m[2][2] * m[3][0];
    t2 = m[2][0] * m[3][3] - m[2][3] * m[3][0];
    t3 = m[2][1] * m[3][2] - m[2][2] * m[3][1];
    t4 = m[2][1] * m[3][3] - m[2][3] * m[3][1];
    t5 = m[2][2] * m[3][3] - m[2][3] * m[3][2];

    d00 =  (t5 * m[1][1] - t4 * m[1][2] + t3 * m[1][3]);
    d10 = -(t5 * m[1][0] - t2 * m[1][2] + t1 * m[1][3]);
    d20 =  (t4 * m[1][0] - t2 * m[1][1] + t0 * m[1][3]);
    d30 = -(t3 * m[1][0] - t1 * m[1][1] + t0 * m[1][2]);

    det = m[0][0] * d00 + m[0][1] * d10 + m[0][2] * d20 + m[0][3] * d30;
	if (det == 0.0f)
		return;

	invDet = 1.0f / det;

    d01 = -(t5 * m[0][1] - t4 * m[0][2] + t3 * m[0][3]);
    d11 =  (t5 * m[0][0] - t2 * m[0][2] + t1 * m[0][3]);
    d21 = -(t4 * m[0][0] - t2 * m[0][1] + t0 * m[0][3]);
    d31 =  (t3 * m[0][0] - t1 * m[0][1] + t0 * m[0][2]);

    t0 = m[1][0] * m[3][1] - m[1][1] * m[3][0];
    t1 = m[1][0] * m[3][2] - m[1][2] * m[3][0];
    t2 = m[1][0] * m[3][3] - m[1][3] * m[3][0];
    t3 = m[1][1] * m[3][2] - m[1][2] * m[3][1];
    t4 = m[1][1] * m[3][3] - m[1][3] * m[3][1];
    t5 = m[1][2] * m[3][3] - m[1][3] * m[3][2];

	d02 =  (t5 * m[0][1] - t4 * m[0][2] + t3 * m[0][3]);
    d12 = -(t5 * m[0][0] - t2 * m[0][2] + t1 * m[0][3]);
    d22 =  (t4 * m[0][0] - t2 * m[0][1] + t0 * m[0][3]);
    d32 = -(t3 * m[0][0] - t1 * m[0][1] + t0 * m[0][2]);

    t0 = m[2][1] * m[1][0] - m[2][0] * m[1][1];
    t1 = m[2][2] * m[1][0] - m[2][0] * m[1][2];
    t2 = m[2][3] * m[1][0] - m[2][0] * m[1][3];
    t3 = m[2][2] * m[1][1] - m[2][1] * m[1][2];
    t4 = m[2][3] * m[1][1] - m[2][1] * m[1][3];
    t5 = m[2][3] * m[1][2] - m[2][2] * m[1][3];

    d03 = -(t5 * m[0][1] - t4 * m[0][2] + t3 * m[0][3]);
    d13 =  (t5 * m[0][0] - t2 * m[0][2] + t1 * m[0][3]);
    d23 = -(t4 * m[0][0] - t2 * m[0][1] + t0 * m[0][3]);
    d33 =  (t3 * m[0][0] - t1 * m[0][1] + t0 * m[0][2]);

	m[0][0] = d00 * invDet;
	m[0][1] = d01 * invDet;
	m[0][2] = d02 * invDet;
	m[0][3] = d03 * invDet;
	m[1][0] = d10 * invDet;
	m[1][1] = d11 * invDet;
	m[1][2] = d12 * invDet;
	m[1][3] = d13 * invDet;
	m[2][0] = d20 * invDet;
	m[2][1] = d21 * invDet;
	m[2][2] = d22 * invDet;
	m[2][3] = d23 * invDet;
	m[3][0] = d30 * invDet;
	m[3][1] = d31 * invDet;
	m[3][2] = d32 * invDet;
	m[3][3] = d33 * invDet;
}

/*
 ==================
 glqMat4::Scale
 ==================
*/
void glqMat4::Scale (const float x, const float y, const float z){

	m[0][0] *= x;
	m[0][1] *= x;
	m[0][2] *= x;
	m[0][3] *= x;
	m[1][0] *= y;
	m[1][1] *= y;
	m[1][2] *= y;
	m[1][3] *= y;
	m[2][0] *= z;
	m[2][1] *= z;
	m[2][2] *= z;
	m[2][3] *= z;
}

/*
 ==================
 glqMat4::Translate
 ==================
*/
void glqMat4::Translate (const float x, const float y, const float z){

	m[3][0] += m[0][0] * x + m[1][0] * y + m[2][0] * z;
	m[3][1] += m[0][1] * x + m[1][1] * y + m[2][1] * z;
	m[3][2] += m[0][2] * x + m[1][2] * y + m[2][2] * z;
	m[3][3] += m[0][3] * x + m[1][3] * y + m[2][3] * z;
}

/*
 ==================
 glqMat4::Shear
 ==================
*/
void glqMat4::Shear (const float x, const float y, const float z){

	glqVec4	mx, my, mz;

	mx[0] = m[0][0] * x;
	mx[1] = m[0][1] * x;
	mx[2] = m[0][2] * x;
	mx[3] = m[0][3] * x;
	my[0] = m[1][0] * y;
	my[1] = m[1][1] * y;
	my[2] = m[1][2] * y;
	my[3] = m[1][3] * y;
	mz[0] = m[2][0] * z;
	mz[1] = m[2][1] * z;
	mz[2] = m[2][2] * z;
	mz[3] = m[2][3] * z;

	m[0][0] += my[0] + mz[0];
	m[0][1] += my[1] + mz[1];
	m[0][2] += my[2] + mz[2];
	m[0][3] += my[3] + mz[3];
	m[1][0] += mx[0] + mz[0];
	m[1][1] += mx[1] + mz[1];
	m[1][2] += mx[2] + mz[2];
	m[1][3] += mx[3] + mz[3];
	m[2][0] += mx[0] + my[0];
	m[2][1] += mx[1] + my[1];
	m[2][2] += mx[2] + my[2];
	m[2][3] += mx[3] + my[3];
}

/*
 ==================
 glqMat4::Rotate
 ==================
*/
void glqMat4::Rotate (const float angle, const float x, const float y, const float z){

	glqVec4	mx, my, mz;
	glqMat3	rotation;
	float	mag, inv, s, c;
	float	x2, y2, z2;
	float	xs, ys, zs;
	float	xy, yz, zx;

	mag = glqMath::Sqrt(x*x + y*y + z*z);
	if (mag == 0.0f)
		return;

	inv = 1.0f / mag;

	x2 = x * inv;
	y2 = y * inv;
	z2 = z * inv;

	glqMath::SinCos(DEG2RAD(angle), s, c);

	inv = 1.0f - c;

	xs = x2 * s;
	ys = y2 * s;
	zs = z2 * s;

	xy = (x2 * y2) * inv;
	yz = (y2 * z2) * inv;
	zx = (z2 * x2) * inv;

	mx[0] = m[0][0];
	mx[1] = m[0][1];
	mx[2] = m[0][2];
	mx[3] = m[0][3];
	my[0] = m[1][0];
	my[1] = m[1][1];
	my[2] = m[1][2];
	my[3] = m[1][3];
	mz[0] = m[2][0];
	mz[1] = m[2][1];
	mz[2] = m[2][2];
	mz[3] = m[2][3];

	rotation[0][0] = (x2 * x2) * inv + c;
	rotation[0][1] = xy + zs;
	rotation[0][2] = zx - ys;
	rotation[1][0] = xy - zs;
	rotation[1][1] = (y2 * y2) * inv + c;
	rotation[1][2] = yz + xs;
	rotation[2][0] = zx + ys;
	rotation[2][1] = yz - xs;
	rotation[2][2] = (z2 * z2) * inv + c;

	m[0][0] = mx[0] * rotation[0][0] + my[0] * rotation[0][1] + mz[0] * rotation[0][2];
	m[0][1] = mx[1] * rotation[0][0] + my[1] * rotation[0][1] + mz[1] * rotation[0][2];
	m[0][2] = mx[2] * rotation[0][0] + my[2] * rotation[0][1] + mz[2] * rotation[0][2];
	m[0][3] = mx[3] * rotation[0][0] + my[3] * rotation[0][1] + mz[3] * rotation[0][2];
	m[1][0] = mx[0] * rotation[1][0] + my[0] * rotation[1][1] + mz[0] * rotation[1][2];
	m[1][1] = mx[1] * rotation[1][0] + my[1] * rotation[1][1] + mz[1] * rotation[1][2];
	m[1][2] = mx[2] * rotation[1][0] + my[2] * rotation[1][1] + mz[2] * rotation[1][2];
	m[1][3] = mx[3] * rotation[1][0] + my[3] * rotation[1][1] + mz[3] * rotation[1][2];
	m[2][0] = mx[0] * rotation[2][0] + my[0] * rotation[2][1] + mz[0] * rotation[2][2];
	m[2][1] = mx[1] * rotation[2][0] + my[1] * rotation[2][1] + mz[1] * rotation[2][2];
	m[2][2] = mx[2] * rotation[2][0] + my[2] * rotation[2][1] + mz[2] * rotation[2][2];
	m[2][3] = mx[3] * rotation[2][0] + my[3] * rotation[2][1] + mz[3] * rotation[2][2];
}

/*
 ==================
 glqMat4::ToMat3
 ==================
*/
glqMat3 glqMat4::ToMat3 (void) const {

	glqMat3	mat;

	mat[0][0] = m[0][0];
	mat[0][1] = m[1][0];
	mat[0][2] = m[2][0];
	mat[1][0] = m[0][1];
	mat[1][1] = m[1][1];
	mat[1][2] = m[2][1];
	mat[2][0] = m[0][2];
	mat[2][1] = m[1][2];
	mat[2][2] = m[2][2];

	return mat;
}

/*
 ==================
 glqMat4::ToString

 This is just a convenience function for printing matrices
 ==================
*/
const char *glqMat4::ToString (int precision) const {

	return Str_FromFloatArray(ToFloatPtr(), 16, precision);
}